/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "absl/memory/memory.h"
#include "slam/mapping/map_builder.h"
#include "slam/transform/rigid_transform.h"
#include "slam/transform/transform.h"
#include "slam_ros/node.h"
#include "slam_ros/node_options.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/utilities.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"

DEFINE_bool(collect_metrics, false,
            "Activates the collection of runtime metrics. If activated, the "
            "metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Slam installation to allow "
              "including files from there.");
DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");
DEFINE_string(load_state_filename, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
            "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
    start_trajectory_with_default_topics, true,
    "Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
    save_state_filename, "",
    "If non-empty, serialize state and write it to disk before shutting down.");

DEFINE_bool(cover_old_map, true, "Whether to overwrite old maps");

DEFINE_string(log_path, "/home/robot/log/localization_log/info/",
              "Default log path");
DEFINE_string(log_warning_path, "/home/robot/log/localization_log/warning/",
              "Default waring log path");

class GLogHelper {
 public:
  GLogHelper(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    // Parse data from input.
    google::ParseCommandLineFlags(&argc, &argv, true);

    std::string path = FLAGS_log_warning_path;
    if (slam_ros::getEnvironmentVariable(path, "LOCATION_LOG_DIR")) {
      LOG(INFO) << "GetEnvironmentVariable LOCATION_LOG_DIR Successfully!";
    } else {
      LOG(INFO) << "GetEnvironmentVariable LOCATION_LOG_DIR Failed!";
    }

    // Add protection for human input error.
    if (path.back() != '/') {
      path += '/';
    }

    if (!slam_ros::dirExists(path)) {
      LOG(ERROR) << "Warning log directory can not be found: " << path;
      LOG(INFO) << "Create log directory: " << slam_ros::mkPath(path, 0777);
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    // Log path can be specified.
    google::SetLogDestination(google::INFO, FLAGS_log_path.c_str());
    google::SetLogDestination(google::WARNING, FLAGS_log_warning_path.c_str());
    LOG(INFO) << "Log path: " << FLAGS_log_path.c_str();
    LOG(WARNING) << "Log warning path: " << FLAGS_log_warning_path.c_str();

    // Let glog output failure.
    google::InstallFailureSignalHandler();
    FLAGS_alsologtostderr = true;
    FLAGS_colorlogtostderr = true;
    FLAGS_max_log_size = 50;
  }
  ~GLogHelper() { google::ShutdownGoogleLogging(); }
};

namespace slam_ros {
namespace {

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
  tf2_ros::Buffer tf_buffer(clock);
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
  node_options.warning_log_path = FLAGS_log_warning_path;
  auto map_builder = absl::make_unique<slam::mapping::MapBuilder>(
      node_options.map_builder_options);
  auto node = std::make_shared<slam_ros::Node>(
      node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);
  if (!FLAGS_load_state_filename.empty()) {
    node->LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  double initial_pose_x = node->get_parameter("initial_pose_x").as_double();
  double initial_pose_y = node->get_parameter("initial_pose_y").as_double();
  double initial_pose_yaw = node->get_parameter("initial_pose_yaw").as_double();
  LOG(INFO) << "initial pose: " << initial_pose_x << " " << initial_pose_y
            << " " << initial_pose_yaw;

  ::slam::mapping::proto::InitialTrajectoryPose initial_trajectory_pose;
  initial_trajectory_pose.set_to_trajectory_id(0);

  const ::slam::transform::Rigid3d initial_pose =
      slam::transform::Embed3D(slam::transform::Rigid2d(
          Eigen::Vector2d(initial_pose_x, initial_pose_y), initial_pose_yaw));
  *initial_trajectory_pose.mutable_relative_pose() =
      slam::transform::ToProto(initial_pose);
  initial_trajectory_pose.set_timestamp(slam::common::ToUniversal(
      ::slam_ros::FromRos(rclcpp::Time(0))));
  *trajectory_options.trajectory_builder_options
       .mutable_initial_trajectory_pose() = initial_trajectory_pose;

  if (FLAGS_start_trajectory_with_default_topics) {
    node->StartTrajectoryWithDefaultTopics(trajectory_options);
  }

  rclcpp::spin(node);

  node->FinishAllTrajectories();
  node->RunFinalOptimization();

  if (!FLAGS_save_state_filename.empty()) {
    node->SerializeState(FLAGS_save_state_filename, true, /*cover_old_map,*/
                        true /* include_unfinished_submaps */);
  }
}

}  // namespace
}  // namespace slam_ros

int main(int argc, char** argv) {
  // google::InitGoogleLogging(argv[0]);
  // google::ParseCommandLineFlags(&argc, &argv, true);
  GLogHelper glog_helper(argc, argv);

  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  ::rclcpp::init(argc, argv);
  
  slam_ros::ScopedRosLogSink ros_log_sink;
  slam_ros::Run();
  ::rclcpp::shutdown();
}
